/**
 * @file monitor.cpp
 * @brief 
 * @author haibo.yang
 * @version 1.1
 * @date 2023-07-26
 * 
 * @copyright Copyright (c) 2023  lgim
 * 
 */
#include <state_monitor/monitor.h>
#include "state_monitor/event_manager.h"
// #include "state_monitor/system_monitor.h"
// #include "state_monitor/node_monitor.h"
#include "state_monitor/safety_monitor.h"
#include "state_monitor/mode_monitor.h"
// #include "state_monitor/ftp_monitor.h"
#include <signal.h>
#include <sys/wait.h>
#include <sys/prctl.h>
#include <fstream>  
#include <sys/stat.h>
// #include "parameters.hpp"
#include <shared_parameters/shared_params_interface.hpp>

int monitor_argc = 0;
char** monitor_argv = nullptr;

namespace monitor {

std::string Monitor::vehicle_model_ = "";
std::shared_ptr<rclcpp::Node> Monitor::node_ = nullptr;
int Monitor::vehicle_safty_state_ = 0;
int Monitor::vehicle_mode_ = 0;
bool Monitor::init_ = false;
std::pair<rclcpp::Time, agv_msgs::msg::ReadInPutsNew> Monitor::inputs;

Monitor::~Monitor()
{}

void Monitor::Tick(const rclcpp::Time current_time) {
  if (next_round_ <= current_time) {
    ++round_count_;
    MONITOR_DEBUG(name_ << " is running round #" << round_count_);
    auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(interval_));
    next_round_ = current_time + rclcpp::Duration(ns);
    RunOnce(current_time);
  }
}

void Monitor::InputCallBack(const agv_msgs::msg::ReadInPutsNew &msg) {
  Monitor::inputs.first = Monitor::GetTimes();
  Monitor::inputs.second = msg;
}

bool Monitor::init_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    MONITOR_INFO( "Failed to initialize shared memory interface");
    return false;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      MONITOR_ERROR("Error get shared parameters");
      return false;
    } 

    params = SharedParamsInterface::getNodeParams("Monitor");
    for (const auto& [name, value] : params) {
      MONITOR_INFO("get param:" << name << ", value:" << value.toString());
      switch(value.type) {
        case SharedParamsInterface::ParamValue::Type::INT:
            Monitor::GetNode()->declare_parameter(name, value.getValue<int>());
            break;
        case SharedParamsInterface::ParamValue::Type::DOUBLE:
            Monitor::GetNode()->declare_parameter(name, value.getValue<double>());
            break;
        case SharedParamsInterface::ParamValue::Type::STRING:
            Monitor::GetNode()->declare_parameter(name, value.getValue<std::string>());
            break;
        case SharedParamsInterface::ParamValue::Type::BOOL:
            Monitor::GetNode()->declare_parameter(name, value.getValue<bool>());
            break;
        default:
            MONITOR_WARN("Unsupported parameter type for " << name);
            break;
      }
    }
  } catch (const std::exception& e) {
    MONITOR_ERROR("Error reading parameters: " << e.what());
    return false;
  }
  return true;
}

}

const char* PID_FILE = "monitor"; 
const char* ROOT_PATH = "/tmp/robotguard/";
std::vector<std::shared_ptr<monitor::Monitor>> monitors_;

bool Proc()
{
  MONITOR_INFO("thread id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));
  while (rclcpp::ok()) {
    auto current_time = monitor::Monitor::GetTimes();
    for (auto& monitor : monitors_) {
      monitor->Tick(current_time);
    }
  
    // Sleep for 10 milliseconds
    boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
  }
  return true;
} 

bool init(std::shared_ptr<rclcpp::Node> node)
{ 
  // monitor::NodeParameters::declareParameters(node.get());
  monitor::Monitor::init_parameters();

  // 2024.11.12 vehcle model
  const char* robot_env = std::getenv("ROBOT");
  std::string robot = robot_env ? robot_env : "None";
  monitor::Monitor::SetVehicleModel(robot);
  
  // 2024.11.11 
  bool simulation_mode = node->get_parameter_or("simulation_mode", false);
  MONITOR_INFO("set simulation mode:" << simulation_mode);

  double cycle = node->get_parameter_or("event_manager_cycle",  0.03);
  // init EventManager
  monitor::EventManager::Instance()->Init(cycle);
  
  // bool usesysmonitor = node->get_parameter_or("/monitor/use_sysmonitor", true);
  // if (usesysmonitor) {
  //   cycle = nh.get_parameter_or("/monitor/system_monitor_cycle", 5.0);
  //   monitors_.emplace_back(new monitor::SystemMonitor(cycle, simulation_mode));
  //   MONITOR_INFO("use_sysmonitor");
  // }

  // bool usenodemonitor = node->get_parameter_or("/monitor/use_nodemonitor", true);
  // if (usenodemonitor) {
  //   cycle = nh.get_parameter_or("/monitor/node_monitor_cycle", 1.0);
  //   monitors_.emplace_back(new monitor::NodeMonitor(cycle, simulation_mode));
  //   MONITOR_INFO("use_nodemonitor");
  // }

  bool usemodemonitor = node->get_parameter_or("use_modemonitor", false);
  if (usemodemonitor) {
    cycle = node->get_parameter_or("mode_monitor_cycle", 1.0);
    monitors_.emplace_back(std::make_shared<monitor::ModeMonitor>(cycle, simulation_mode));
    MONITOR_INFO("use_modemonitor:" << cycle);
  }

  bool usesaftymonitor = node->get_parameter_or("use_internal_safty_moudle", true);
  if (usesaftymonitor) {
    cycle = node->get_parameter_or("safety_monitor_cycle", 0.1);
    monitors_.emplace_back(std::make_shared<monitor::SafetyMonitor>(cycle, simulation_mode));
    MONITOR_INFO("use_internal_safty_moudle:" << cycle);
  }

  // monitors_.emplace_back(new monitor::FtpMonitor(nh, 1, simulation_mode));
  
  // init monitor
  for (auto & monitor : monitors_) {
    monitor->init();
  }

  return true;
}

int monitor_process(int argc, char** argv) 
{
  // Prevent zombie processes, when ..
  prctl(PR_SET_PDEATHSIG, SIGHUP);
  rclcpp::init(argc, argv);
  monitor::Monitor::node_ = rclcpp::Node::make_shared("monitor");
  MONITOR_INFO("Start sub monitor process:" << getpid() << " Guard:" << getppid());
  init(monitor::Monitor::GetNode());
  
  // MONITOR_INFO("Start sub monitor process:" << getpid() << " Guard:" << getppid());
  boost::thread monitor_thread = boost::thread(boost::bind(&Proc));

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(monitor::Monitor::GetNode());
  executor.spin();
  
  MONITOR_INFO("End monitor.");
  monitor_thread.join();
  rclcpp::shutdown();

  return 0;
}

// 
void watchdog_handler(int signal) 
{
  pid_t monitor_pid;
  int status;

  // Wait for the monitor process to exit
  monitor_pid = waitpid(-1, &status, WNOHANG);

  bool reboot = false;
  if (monitor_pid > 0) {
    if (WIFEXITED(status)) {
      MONITOR_ERROR("monitor process:" << monitor_pid << " exit, exit code:" << WEXITSTATUS(status));
    } else if (WIFSIGNALED(status)) {
      MONITOR_ERROR("monitor process:" << monitor_pid << " be terminated, signal:" << WTERMSIG(status));
    } else if (WIFSTOPPED(status)) {
      MONITOR_WARN("moinitor process stopped by signal:" << WSTOPSIG(status));
    } else if (WIFCONTINUED(status)) {
      MONITOR_WARN("monitor process continued");
    } else {
      MONITOR_ERROR("monitor process unknown status");
    }

    reboot = true;
  } else if (monitor_pid == -1) {
    MONITOR_ERROR("waitpid error:" << errno);
    // Abnormal exit and reboot
    if (errno == ECHILD) { 
      reboot = true;
    }
  }

  if (reboot) {
    // reboot monitor process
    MONITOR_WARN("Restart the monitor process:" << monitor_pid);
    
    if ((monitor_pid = fork()) == 0) {
      monitor_process(monitor_argc, monitor_argv);
      exit(0);
    } 
  }
}
 
bool is_process_running(pid_t pid) {  
    if (pid <= 0) return false;  
    int ret = kill(pid, SIGINT);
    if (ret == -1 || errno == EPERM) {  
        // ROS_WARN_STREAM("Kill :" << ret << ", errno:" << errno);
        // Process exists, or we don't have permission to send signal (but it exists)  
        return true;  
    }  
    // Process does not exist  
    return false;  
}  
  
bool create_pid_file(pid_t pid) {  
    struct stat info;
    if (stat(ROOT_PATH, &info) != 0) {
      if (mkdir(ROOT_PATH, 0755) != 0) {
        MONITOR_WARN("Create dir: " << ROOT_PATH << "failed!");
      } else {
        MONITOR_INFO("Create dir: " << ROOT_PATH << "succeed!");
      }
    }

    std::ofstream pid_file(std::string(ROOT_PATH) + std::string(PID_FILE));  
    if (!pid_file.is_open()) {  
        MONITOR_WARN("Failed to open PID file for writing.");  
        return false;  
    }  
    pid_file << pid;  
    pid_file.close();  
    return true;  
}  
  
bool check_and_create_file(pid_t pid) {  
    std::ifstream pid_file(PID_FILE);  
    pid_t existing_pid;  
    if (pid_file.is_open()) {  
        // Read the existing PID from the file  
        pid_file >> existing_pid;  
        pid_file.close();  
  
        // Check if the process with that PID is still running  
        if (is_process_running(existing_pid)) {  
            std::cout <<  "Another process with PID " << existing_pid << " is already running." << std::endl;  
            return false; // Exit the new process  
        }  
  
        // The existing process is not running, so remove the old PID file  
        remove(PID_FILE);  
    }  
  
    // Create the new PID file  
    return create_pid_file(pid);  
}  

int main(int argc, char** argv) 
{
  pid_t pid;
  bool watch_process = true;
  int milliseconds = 2000;
  
  if (!check_and_create_file(getpid())) {
    std::cerr << "Can't create new monitor process. exit" << std::endl;
    return 1;
  }

  monitor_argc = argc;
  monitor_argv = argv;

  // create monitor process
  if ((pid = fork()) == 0) {
    // MONITOR_INFO("monitor process:" << pid);
    monitor_process(monitor_argc, monitor_argv);
    exit(0);
  } else {
    MONITOR_INFO("Guard monitor process:" << getpid());
  }

  while (watch_process) {
    // set the signal processing function
    signal(SIGCHLD, watchdog_handler);
    usleep(milliseconds);
  }

  // Wait for the child process to exit
  waitpid(pid, NULL, 0);
  remove(PID_FILE); 
  return 0;
}
